#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <vector>
#include <opencv2/opencv.hpp>
#include "dragon.h"
#include "mynode_cv.h"
#include <ctime>
const int samplesize = 50000;
void ResconstructTree(Eigen::SparseMatrix<unsigned char, Eigen::RowMajor>& E,
                      cv::KDTree& tree, int fatherid, std::vector<myNodeCv>& node_details)
{
    if(tree.nodes[fatherid].idx<0){
        //如果遇到了叶子
        node_details[fatherid].Es = E.row(std::abs(tree.nodes[fatherid].idx) -1).cast<float>();
        node_details[fatherid].Pv[0] = vertices[std::abs(tree.nodes[fatherid].idx)-1][0];
        node_details[fatherid].Pv[1] = vertices[std::abs(tree.nodes[fatherid].idx)-1][1];
        node_details[fatherid].Pv[2] = vertices[std::abs(tree.nodes[fatherid].idx)-1][2];
        node_details[fatherid].point_count = 1;
        return;
    }
    ResconstructTree(E, tree, tree.nodes[fatherid].left, node_details);
    ResconstructTree(E, tree, tree.nodes[fatherid].right, node_details);
    int leftchild = tree.nodes[fatherid].left,
            rightchild = tree.nodes[fatherid].right;
    //sum of E
    node_details[fatherid].Es = node_details[leftchild].Es + node_details[rightchild].Es;
    // pointmax and pointmin
    node_details[fatherid].point_count =  node_details[leftchild].point_count + node_details[rightchild].point_count;

    //pv
    //计算左右权重
    //左权重
    Eigen::SparseMatrix<float, Eigen::RowMajor>::InnerIterator it_left(node_details[leftchild].Es, 0);
    float p_left = 0;
    while(it_left){
        p_left += it_left.value() * it_left.value();
        ++it_left;
    }
    p_left = sqrt(p_left);
    Eigen::SparseMatrix<float, Eigen::RowMajor>::InnerIterator it_right(node_details[rightchild].Es, 0);
    //右权重
    float p_right = 0;
    while(it_right){
        p_right += it_right.value() * it_right.value();
        ++it_right;
    }
    p_right = sqrt(p_right);
    for(int i=0; i<3; i++){
        node_details[fatherid].Pv[i] = (node_details[leftchild].Pv[i] * p_left +node_details[rightchild].Pv[i] * p_right)
                / (p_left + p_right);
    }
    return;
}
const float PI = 3.1415;
float getR(float* x0, float* x1)
{
    static float yita = 1.5;
    static float sigma_s = 0.75, sigma_a = 0.02;
    static float g=0.25;//散射角平均cos
    static float sigma_s2 = (1-g)*sigma_s, sigma_t2 = sigma_a+sigma_s2,
            alpha2 = sigma_s2/sigma_t2, sigma_tr = sqrt(3*sigma_a*sigma_t2), z_r = 1/sigma_t2,
            F_dr = -1.440/yita*yita+0.710/yita+0.668+0.0636*yita, A = (1+F_dr)*(1-F_dr);
    float r = sqrt(pow(x1[0]-x0[0], 2)+
            pow(x1[1]-x0[1], 2)+
            pow(x1[2]-x0[2], 2));
    float d_r = sqrt(r*r + z_r*z_r),z_v = z_r*(1+4*A/3), d_v = sqrt(r*r +z_v*z_v);
    float R;
    R = alpha2/(4*PI)*(z_r*(sigma_tr+1/d_r)*exp(-sigma_tr*d_r)/d_r/d_r +
                       z_v*(sigma_tr+1/d_v)*exp(-sigma_tr*d_v)/d_v/d_v);
    return R;
}
const float thresold = 0.5f;
Eigen::SparseMatrix<float, Eigen::RowMajor> SearchTree(
        float curx[],
                 int fatherid, int x0, bool ownX, cv::KDTree& tree,
                 std::vector<myNodeCv>& node_details)
{
    if(tree.nodes[fatherid].idx<0){
        //突破重围居然到了叶子。。。看来是个离x0很近的点，直接算
        float R = getR(curx, node_details[fatherid].Pv);
        return R*node_details[fatherid].Es;
    }
    //如果x在当前左子树，继续分左边
    if(ownX && curx[tree.nodes[fatherid].idx]<=tree.nodes[fatherid].boundary){
        return SearchTree(curx, tree.nodes[fatherid].right, x0, false, tree, node_details) +
                SearchTree(curx, tree.nodes[fatherid].left, x0, true, tree, node_details);
    }else if(ownX && curx[tree.nodes[fatherid].idx]>tree.nodes[fatherid].boundary){
        return SearchTree(curx, tree.nodes[fatherid].right, x0, true, tree, node_details) +
                SearchTree(curx, tree.nodes[fatherid].left, x0, false, tree, node_details);
    }
    //如果当前小于阈值，返回R
    float www =1.0f;
    float area = node_details[fatherid].point_count;
    float deter = sqrt(
                pow(curx[0] - node_details[fatherid].Pv[0], 2)
                +pow(curx[1] - node_details[fatherid].Pv[1], 2)
                +pow(curx[2] - node_details[fatherid].Pv[2], 2));
    if(!deter){
        std::cout<<"fuck"<<std::endl;
    }
    www = area / deter;
    //std::cout<<www<<std::endl;
    if(www<thresold){
        //小于积分项阈值，计算
        float R = getR(curx, node_details[fatherid].Pv);
        return R*node_details[fatherid].Es;
    }
    return SearchTree(curx, tree.nodes[fatherid].right, x0, false, tree, node_details) +
            SearchTree(curx, tree.nodes[fatherid].left, x0, false, tree, node_details);
}
void GetT(Eigen::SparseMatrix<float, Eigen::RowMajor>& T,
          std::vector<myNodeCv>& node_details, cv::KDTree& tree)
{
    for(int i=0; i< T.rows(); i++){
        //std::cout<<clock()/CLOCKS_PER_SEC<<std::endl;
        float* curx = vertices[i];
        T.row(i) = SearchTree(curx, 0, i, true, tree, node_details);
        std::cout<<clock()/CLOCKS_PER_SEC<<std::endl;
        //Eigen::SparseMatrix<float, Eigen::RowMajor>::InnerIterator it(T, i);
//        while(it){
//            float value = it.value();
//            std::cout<<value<<std::endl;
//            ++it;
//        }
    }
}

int main()
{
    cv::Mat curE(samplesize, 128,CV_8UC1);
    std::ifstream in("waveE.txt", std::ios::binary);
    std::cout<<"open"<<std::endl;
    in.read((char*)curE.data, curE.rows * curE.cols * sizeof(uchar));
    cv::Mat maxid(samplesize, 128, CV_32SC1);
    std::ifstream in2("maxid.txt", std::ios::binary);
    std::cout<<"open"<<std::endl;
    in2.read((char*)maxid.data, maxid.rows * maxid.cols * sizeof(int));
    std::cout<<maxid.row(0)<<std::endl;
    Eigen::SparseMatrix<unsigned char, Eigen::RowMajor> sparseE_(samplesize,6*32*32);
    std::vector<Eigen::Triplet<unsigned char> > triple;
    for(int i=0;i<samplesize;i++){
        int* p = maxid.ptr<int>(i);
        uchar*p1 = curE.ptr<uchar>(i);
        for(int j=0;j<128;j++){
             //sparseE_.coeffRef(i,*p) = *p1;
            triple.push_back(Eigen::Triplet<unsigned char>(i, *p, *p1));
            p++;
            p1++;
        }
    }
    sparseE_.setFromTriplets(triple.begin(), triple.end());
    maxid.release();

    cv::Mat vertices_mat(50000, 3, CV_32FC1/*, vertices*/);
    memcpy(vertices_mat.data, &vertices[0][0], vertices_mat.rows * vertices_mat.cols * sizeof(float));
    cv::KDTree tree(vertices_mat);
    std::vector<myNodeCv> node_details(tree.nodes.size());
    ResconstructTree(sparseE_, tree, 0, node_details);
    Eigen::SparseMatrix<float, Eigen::RowMajor> T(samplesize, 6*32*32);
    std::cout<<"begin"<<std::endl;
    GetT(T, node_details, tree);
    int balala = T.nonZeros();
    std::ofstream outttt("T_dasibugai.txt", std::ios::binary);
    outttt.write((char*)&balala, sizeof(int));
    for(int i=0; i<T.rows();i++){
        Eigen::SparseMatrix<float, Eigen::RowMajor>::InnerIterator it(T, i);
        while(it){
            float value = it.value();
            int row = it.row(), col = it.col();
            outttt.write((char*)&value, sizeof(float));
            outttt.write((char*)&row, sizeof(int));
            outttt.write((char*)&col, sizeof(int));
            ++it;
        }
    }
}
